#include <iostream>
#include "apm_flight_handler.h"
#include "mount_siyi.h"
#include "assist_land.h"


using namespace std;


void siyiUpdateHandler();
void siyiRtspHandler();
void asssitLandHandler();



int main(int argc, char **argv)
{
	bool ret;

	cout << "start siyi update handler...." << endl;
	//start siyi update
	std::thread siyiUpdate(siyiUpdateHandler);

	cout << "start siyi rtsp handler...." << endl;
	//start siyi rtsp
	std::thread siyiRtsp(siyiRtspHandler);

	cout << "start flight handler...." << endl;

	ret = flightHandler.startUpSystem(&mountSiyiHandler);
	if(!ret)
	{
		cout << " startUp system error!!!" << endl;
		return -1;
	}

	cout << "startUp system ok" << endl;

	//start assist land mode
	std::thread assistLandThread(asssitLandHandler);

	bool statusLedFlag;

	while(1)
	{
#ifdef PLATFORM_ARCH_aarch64
		statusLedFlag = !statusLedFlag;
		if(statusLedFlag)
		{
			system("echo 0 > /sys/class/gpio/gpio21/value");
		}
		else
		{
			system("echo 1 > /sys/class/gpio/gpio21/value");
		}
#endif

		sleepMs(1000);

		//write the cpu temperature to log
		flightHandler.recodeCpuTemperature();
	}

	siyiUpdate.join();
	siyiRtsp.join();
	assistLandThread.join();
	return 0;
}


void siyiUpdateHandler()
{
	bool ret = mountSiyiHandler.init("udp", "192.168.10.128");
	if(!ret)
	{
		printf("siyi init error\n");
		return;
	}
	mountSiyiHandler.update();
}


void siyiRtspHandler()
{
	bool ret = mountSiyiHandler.startRtspRecv("rtsp://192.168.10.128:8554/main.264");
	if(!ret)
	{
		printf("start rtsp recv error\n");
		return;
	}
}


void asssitLandHandler()
{
	assistLand.init(&flightHandler);
	assistLand.assistLandRun();
}

